// --------------------------------------------------------------------------------------------------------------------
// <copyright file="KinematicChainTests.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Kinematics
{
    using System;
    using System.Collections.Generic;
    using Microsoft.Robotics.Kinematics;
    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;
    using ShoNS.Array;

    /// <summary>
    /// Test kinematic chain class
    /// </summary>
    [TestClass]
    [DeploymentItem(@"ShoArray.dll")]
    [DeploymentItem(@"MatrixInterf.dll")]
    [DeploymentItem(@"ArrayT.dll")]
    [DeploymentItem(@"libiomp5md.dll")]
    [DeploymentItem(@"mkl.dll")]
    [DeploymentItem(@"msvcp110.dll")]
    [DeploymentItem(@"msvcr110.dll")]
    public class KinematicChainTests
    {
        /// <summary>
        /// Create a 7DOF kinematic chain
        /// </summary>
        /// <returns>A kinematic chain</returns>
        public static KinematicChain CreateA7DOFKinematicChain()
        {
            int numJoints = 7;

            // create the joint list
            Joint[] jointList = new Joint[numJoints];
            jointList[0] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(0, 0, 310.5), 0, -170.0 * MathConstants.Degrees2Radians, 170.0 * MathConstants.Degrees2Radians);
            jointList[1] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(MathConstants.PIOverTwo, 0, 0), MathConstants.PIOverTwo, -120.0 * MathConstants.Degrees2Radians, 120.0 * MathConstants.Degrees2Radians);
            jointList[2] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(MathConstants.PIOverTwo, 0, 400), 0, -170.0 * MathConstants.Degrees2Radians, 170.0 * MathConstants.Degrees2Radians);
            jointList[3] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(MathConstants.PIOverTwo, 0, 0), 0, -120.0 * MathConstants.Degrees2Radians, 120.0 * MathConstants.Degrees2Radians);
            jointList[4] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(-MathConstants.PIOverTwo, 0, 390), 0, -170.0 * MathConstants.Degrees2Radians, 170.0 * MathConstants.Degrees2Radians);
            jointList[5] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(-MathConstants.PIOverTwo, 0, 0), 0, -120.0 * MathConstants.Degrees2Radians, 120.0 * MathConstants.Degrees2Radians);
            jointList[6] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(MathConstants.PIOverTwo, 0, 0), 0, -170.0 * MathConstants.Degrees2Radians, 170.0 * MathConstants.Degrees2Radians);

            // create a kinematic chain
            KinematicChain kc = new KinematicChain(jointList);

            // where the EE is w.r.t. the last joint
            Matrix4 endEffectorInLastJoint = HomogenousTransform.CreateFromTranslation(new Vector3(0, 0, 217.5));
            endEffectorInLastJoint[0, 0] = -1;
            endEffectorInLastJoint[1, 1] = -1;
            kc.SetEndEffectorOffsetInLastJoint(endEffectorInLastJoint);

            return kc;
        }

        /// <summary>
        /// Tests the joint query
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void KinematicsJointQueryTestKuka7DOF()
        {
            // create an kinematic chain for a 7-DOF Kuka arm
            KinematicChain kc = CreateA7DOFKinematicChain();

            for (int i = 0; i < 100; ++i)
            {
                // generate one joint sample
                DoubleArray jointSample = InverseKinematicsNumerical.GenerateRandomJointConfiguration(kc, i + 1);

                // update joint values
                kc.UpdateJointValues(jointSample);

                // query joint values
                double[] jointQueried = kc.GetJointValues();

                // validate
                CollectionAssert.AreEqual(jointSample, jointQueried);
            }
        }

        /// <summary>
        /// Tests consistency of the forward kinematics for two routes.
        /// First route is update the kinematic chain and the second route
        /// is passing joint values directly when querying for joint transforms.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ForwardKinematicsConsistencyTestKuka7DOF()
        {
            // create an kinematic chain for a 7-DOF Kuka arm
            KinematicChain kc = CreateA7DOFKinematicChain();

            for (int i = 0; i < 100; ++i)
            {
                // generate one joint sample
                DoubleArray jointSample = InverseKinematicsNumerical.GenerateRandomJointConfiguration(kc, i + 1);

                // calculate end effector pose using forward kinematics
                Matrix4[] kinematics1 = kc.GetJointTransforms(jointSample);
                kc.UpdateJointValues(jointSample);
                Matrix4[] kinematics2 = kc.GetJointTransforms();

                PoseDiff diff = new PoseDiff(kinematics1[6], kinematics2[6]);
                Assert.AreEqual(MatrixExtensions.MaxAbsoluteValue(diff), 0, 1e-3);
            }
        }

        /// <summary>
        /// Tests forward kinematics using the 7-DOF Kuka arm
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ForwardKinematicsTestKuka7DOF()
        {
            // create an kinematic chain for a 7-DOF Kuka arm
            KinematicChain kc = CreateA7DOFKinematicChain();

            // generate one joint sample
            double[] jointSample0N = { 0, MathConstants.PIOverTwo, 0, 0, 0, 0, 0 };
            double[] jointSample1N = { 0, MathConstants.PIOverTwo, 0, MathConstants.PIOverTwo, 0, 0, 0 };
            double[] jointSample2N = { 0, MathConstants.PIOverTwo, MathConstants.PIOverFour, MathConstants.PIOverTwo, 0, 0, 0 };
            double[] jointSample3N = { 0, 120 * MathConstants.Degrees2Radians, MathConstants.PIOverFour, MathConstants.PIOverTwo, 0, 0, 0 };
            double[] jointSample4N = { 0, 120 * MathConstants.Degrees2Radians, MathConstants.PIOverFour, MathConstants.PIOverTwo, 0, 30 * MathConstants.Degrees2Radians, 0 };
            double[] jointSample5N = { 33.98 * MathConstants.Degrees2Radians, 84.59 * MathConstants.Degrees2Radians, -18.92 * MathConstants.Degrees2Radians, 34.00 * MathConstants.Degrees2Radians, -18.72 * MathConstants.Degrees2Radians, 17.54 * MathConstants.Degrees2Radians, -61.80 * MathConstants.Degrees2Radians };

            DoubleArray jointSample0 = DoubleArray.From(jointSample0N).Transpose();
            DoubleArray jointSample1 = DoubleArray.From(jointSample1N).Transpose();
            DoubleArray jointSample2 = DoubleArray.From(jointSample2N).Transpose();
            DoubleArray jointSample3 = DoubleArray.From(jointSample3N).Transpose();
            DoubleArray jointSample4 = DoubleArray.From(jointSample4N).Transpose();
            DoubleArray jointSample5 = DoubleArray.From(jointSample5N).Transpose();

            // calculate end effector pose 0 using forward kinematics
            Matrix4 endEffectorPose0 = kc.GetEndEffectorTransform(jointSample0);
            Pose pose0 = new Pose(new Vector3(0, 0, 1318), new Quaternion(0, 0, 0, 1));

            // calculate end effector pose 1 using forward kinematics
            Matrix4 endEffectorPose1 = kc.GetEndEffectorTransform(jointSample1);
            Pose pose1 = new Pose(new Vector3(607.5, 0, 710.5), new Quaternion(HomogenousTransform.CreateFromEulerAngleZYX(141.81 * MathConstants.Degrees2Radians, MathConstants.PIOverTwo, 141.81 * MathConstants.Degrees2Radians)));

            // calculate end effector pose 2 using forward kinematics
            Matrix4 endEffectorPose2 = kc.GetEndEffectorTransform(jointSample2);
            Pose pose2 = new Pose(new Vector3(429.567, 429.567, 710.5), new Quaternion(HomogenousTransform.CreateFromEulerAngleZYX(-166.75 * MathConstants.Degrees2Radians, MathConstants.PIOverTwo, 148.25 * MathConstants.Degrees2Radians)));

            // calculate end effector pose 3 using forward kinematics
            kc.UpdateJointValues(jointSample3);
            Matrix4 endEffectorPose3 = kc.GetEndEffectorTransform();
            Pose pose3 = new Pose(new Vector3(172.016, 429.567, 871.694), new Quaternion(HomogenousTransform.CreateFromEulerAngleZYX(0, 60 * MathConstants.Degrees2Radians, -45 * MathConstants.Degrees2Radians)));

            // calculate end effector pose 4 using forward kinematics
            kc.UpdateJointValues(jointSample4);
            Matrix4 endEffectorPose4 = kc.GetEndEffectorTransform();
            Pose pose4 = new Pose(new Vector3(99.797, 408.96, 955.57), new Quaternion(HomogenousTransform.CreateFromEulerAngleZYX(25.57 * MathConstants.Degrees2Radians, 34.97 * MathConstants.Degrees2Radians, -25.56 * MathConstants.Degrees2Radians)));

            // calculate end effector pose 5 using forward kinematics
            kc.UpdateJointValues(jointSample5);
            Matrix4 endEffectorPose5 = kc.GetEndEffectorTransform();
            Pose pose5 = new Pose(new Vector3(339.11, 142.062, 1210.483), new Quaternion(HomogenousTransform.CreateFromEulerAngleZYX(-63.23 * MathConstants.Degrees2Radians, -2.85 * MathConstants.Degrees2Radians, -23.42 * MathConstants.Degrees2Radians)));

            // compare
            PoseDiff diff0 = new PoseDiff(endEffectorPose0, HomogenousTransform.CreateFromPose(pose0));
            PoseDiff diff1 = new PoseDiff(endEffectorPose1, HomogenousTransform.CreateFromPose(pose1));
            PoseDiff diff2 = new PoseDiff(endEffectorPose2, HomogenousTransform.CreateFromPose(pose2));
            PoseDiff diff3 = new PoseDiff(endEffectorPose3, HomogenousTransform.CreateFromPose(pose3));
            PoseDiff diff4 = new PoseDiff(endEffectorPose4, HomogenousTransform.CreateFromPose(pose4));
            PoseDiff diff5 = new PoseDiff(endEffectorPose5, HomogenousTransform.CreateFromPose(pose5));

            // verify
            Assert.AreEqual(MatrixExtensions.MaxAbsoluteValue(diff0), 0, 1e-2, "1");
            Assert.AreEqual(MatrixExtensions.MaxAbsoluteValue(diff1), 0, 1e-2, "2");
            Assert.AreEqual(MatrixExtensions.MaxAbsoluteValue(diff2), 0, 1e-2, "3");
            Assert.AreEqual(MatrixExtensions.MaxAbsoluteValue(diff3), 0, 1e-2, "4");
            Assert.AreEqual(MatrixExtensions.MaxAbsoluteValue(diff4), 0, 1e-2, "5");
            Assert.AreEqual(MatrixExtensions.MaxAbsoluteValue(diff5), 0, 1e-2, "6");
        }
    }
}
